package org.nashua.tt151.systems;

import edu.wpi.first.wpilibj.Relay;
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.templates.Dash;
import org.nashua.tt151.libraries.Controller.DualAction;

/**
 * 2014 Camera Control System
 * 
 * @author Kareem El-Faramawi
 */
public class Camera extends Subsystem {
	private static Camera INSTANCE;
	private static final Servo vertical = new Servo( 10 );
	private static final Relay LED = new Relay( 1, Relay.Direction.kForward );
	
	private double camVertical = 0.5;

	private final double VERTICAL_DEFAULT = 54.97872340425532;
	private final double TARGET_HEIGHT = 54;
	private final double LENS_HEIGHT = 10.5625;
	private double hypot = 0.0;
	private double dist = 0.0;
	private boolean autoAngle = false;
	private Camera() {
	}

	public static Camera getInstance() {
		if ( INSTANCE == null ) {
			INSTANCE = new Camera();
		}
		return INSTANCE;
	}

	public void init() {
		resetServo();
		LED.set( Relay.Value.kOn );
	}
	
	boolean rave = false;
	long lastBlink = System.currentTimeMillis();

	public void operatorControl( DualAction driver, DualAction shooter ) {
		turn( 0.0, Dash.camTurnVert );
		turn( driver.getRawAxis( DualAction.Axis.DPAD_X ) * 0.0005, driver.getRawAxis( DualAction.Axis.DPAD_Y ) * 0.0005 );
		turn( shooter.getRawAxis( DualAction.Axis.DPAD_X ) * 0.0005, shooter.getRawAxis( DualAction.Axis.DPAD_Y ) * 0.0005 );
		if ( driver.wasReleased( DualAction.Button.START ) || shooter.wasReleased( DualAction.Button.START ) ) {
			LED.set( LED.get() == Relay.Value.kOff ? Relay.Value.kOn : Relay.Value.kOff );
		}
		double angle = vertical.getAngle() - VERTICAL_DEFAULT;
		hypot = ( ( TARGET_HEIGHT - LENS_HEIGHT ) / Math.sin( Math.toRadians( angle ) ) );
		dist = ( ( TARGET_HEIGHT - LENS_HEIGHT ) / Math.tan( Math.toRadians( angle ) ) );
//		System.out.println( "Vertical: " + vertical.getAngle() + "\t\tHorizontal: " + horizontal.getAngle() );
//		System.out.println( angle + "\tHYPOT: " + hypot + " in\tDIST: " + dist + " in" );
	}

	public double getHypot() {
		return hypot;
	}

	public double getDist() {
		return dist;
	}

	private void turn( double horiz, double vert ) {
		camVertical += vert;
		camVertical = Math.max( 0, Math.min( 1, camVertical ) );
		vertical.setPosition( camVertical );
	}

	public void resetServo() {
		setServoAngle( VERTICAL_DEFAULT );
	}

	public void setServo( double angle ) {
		vertical.setAngle( angle );
		camVertical = vertical.get();
	}

	public void setServoAngle( double value ) {
		vertical.setAngle( value );
		camVertical = vertical.get();
	}
}
